#include "dcmotor.h"
#include "lib/controlacc.h"
#include "fwapplication.h"

#define DCMOTOR_PWM_DEFAULT_V0 10
#define DCMOTOR_PWM_DEFAULT_VMAX 100
#define DCMOTOR_PWM_DEFAULT_SPEEDUS (300000)

DcMotor::DcMotor()
{
}

DcMotor::DcMotor(const GpioPortPin &backPortPin, const GpioPortPin &frontPortPin)
{
    Config(backPortPin, frontPortPin);
}

DcMotor::~DcMotor()
{
    Stop();
}

void DcMotor::Config(const GpioPortPin &backPortPin, const GpioPortPin &frontPortPin)
{
    m_backPort = backPortPin;
    m_frontPort = frontPortPin;
}

void DcMotor::AttachPwm(SoftUpdatePwm *pwm, int duty0, int dutyMax, int speedUs)
{
    m_pwm = pwm;
    if(m_pwm != nullptr)
    {
        m_pwm->SetFinishedCallback([=](SoftUpdatePwm &sender){
            m_pwm->SetDuty(m_accer.Update(sender.GetPrevPointUs()));
        });
    }
    m_accer.Config(duty0, dutyMax, speedUs);
}

void DcMotor::DettachPwm()
{
    m_pwm = nullptr;
    m_accer.Reset();
}

DeviceDirection DcMotor::GetCurrentDirection()
{
    return m_currentDir;
}

bool DcMotor::IsRunning()
{
    return m_isRunning;
}

void DcMotor::Start(DeviceDirection dir)
{
    StartImpl(dir, 0);
}

void DcMotor::ReStart(DeviceDirection dir)
{
    Stop();
    Start(dir);
}

void DcMotor::StartTime(DeviceDirection dir, int ms)
{
    StartImpl(dir, ms);
}

void DcMotor::StartTimeWithSumAndAutoDir(DeviceDirection dir, int ms)
{
    //换向了
    if(m_isRunning && m_currentDir != dir)
    {
        Stop();
    }

    if(m_isRunning)
    {
        m_remainsRunTime += ms;
    }
    else
    {
        StartTime(dir, ms);
    }
}

void DcMotor::Stop()
{
    if(!m_isRunning)
        return;

    Control(DeviceDirection::None);
}

int DcMotor::GetRunningTimeMs()
{
    return m_runningMs;
}

void DcMotor::StartImpl(DeviceDirection dir, int ms)
{
    if(m_isRunning)
        return;

    m_remainsRunTime = ms;
    Control(dir);
}

void DcMotor::StartTimer()
{
    if(m_timer.IsRunning())
        m_timer.Stop();
        
    m_timer.SetInterval(DCMOTOR_TIMER_TICK_MS);
    m_timer.SetElapsed([=](auto &timer){FW_UNUSED(timer); TimerHandler();});
    m_timer.Start(); 
}

void DcMotor::TimerHandler()
{
    if(m_isRunning)
    {
        m_runningMs += m_timer.GetInterval();
    }

    if(m_remainsRunTime > 0)
    {
        m_remainsRunTime -= m_timer.GetInterval();
        if(m_remainsRunTime <= 0)
        {
            m_remainsRunTime = 0;
            Stop();
        }
    }
}

void DcMotor::Control(DeviceDirection dir)
{
    StartTimer();
 
    m_runningMs = 0;
    switch(dir)
    {
        case DeviceDirection::CW:
            {
                ControlImpl(m_frontPort, m_backPort);
                m_isRunning = true;
            }
            break;
        case DeviceDirection::CCW:
            {
                ControlImpl(m_backPort, m_frontPort);
                m_isRunning = true;
            }
            break;
        case DeviceDirection::None: //stop
        default:
            {
                SOC_Gpio::Set(m_backPort);
                SOC_Gpio::Set(m_frontPort);
                m_isRunning = false;
            }
            break;
    }

    m_currentDir = dir;
}

void DcMotor::ControlImpl(const GpioPortPin &pwmPortPin, const GpioPortPin &lowPortPin)
{
    if(m_pwm == nullptr)
    {
        SOC_Gpio::Set(pwmPortPin);
    }
    else
    {
        if(m_accer.IsEmptyConfig())
        {
            m_accer.Config(DCMOTOR_PWM_DEFAULT_V0, DCMOTOR_PWM_DEFAULT_VMAX, DCMOTOR_PWM_DEFAULT_SPEEDUS);
        }
        
        m_accer.StartAcc();
        m_pwm->AttachGpio(pwmPortPin);
        m_pwm->SetDuty(m_accer.Update(0));
        m_pwm->Start();
    }

    SOC_Gpio::Clear(lowPortPin);
}
